perm filename ARMSOL[SYS,HE]1 blob
sn#069206 filedate 1973-11-08 generic text, type T, neo UTF8
COMMENT ⊗ VALID 00014 PAGES
RECORD PAGE DESCRIPTION
00001 00001
00003 00002 TITLE ARMSOL
00004 00003 BEGIN CROSS
00005 00004 BEGIN SECOND
00006 00005 BEGIN ANGLE
00008 00006 BEGIN SOLVE
00012 00007 WE NOW NEED SOME SINES AND COSINES
00014 00008 NOW JOINT 4
00017 00009 DEGEN: SETZM J+4 JOINT 5 IS ZERO
00018 00010 FLAG: 0
00019 00011 BEGIN UPDATE
00022 00012 MUL: MOVEI J,2
00023 00013 TH: BLOCK 6
00024 00014 BEGIN TRANSMULT
00026 ENDMK
⊗;
TITLE ARMSOL
;THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS
INTERNAL ANGLE,SOLVE,UPDATE,TIMES
EXTERNAL ACOS,SQRT,ATAN2,ASIN,ACOS,SIND,COSD
↓AC←1
↓P←17
S2V←←6.05
S22V←←36.6025
S6V←←9.38
↓RAD: 206712273406
↓SHOLDER:20.035
10.305
16.24
BEGIN CROSS
;CROSS(R,P,A) RETURNS THE CROSS PRODUCT IN R
;TIME APPROX 180 MICRO SECONDS
TAC1←←2
PV1←←3
PV2←←4
PV3←←5
AV1←←6
AV2←←7
AV3←←10
O←←11
↑CROSS:
HRL AC,-2(P) ;MOVE PV INTO AC'S
HRRI AC,PV1
BLT AC,PV3
HRL AC,-1(P) ;MOVE AV INTO AC'S
HRRI AC,AV1
BLT AC,AV3
MOVE O,-3(P)
MOVE AC,PV2
FMPR AC,AV3
MOVE TAC1,PV3
FMPR TAC1,AV2
FSBR AC,TAC1
MOVEM AC,(O)
FMPR PV3,AV1
FMPR AV3,PV1
FSBR PV3,AV3
MOVEM PV3,1(O)
FMPR PV1,AV2
FMPR AV1,PV2
FSBR PV1,AV1
MOVEM PV1,2(O)
SUB P,[4(4)]
JRST @4(P)
BEND
BEGIN SECOND
;SECOND(A) RETURNS THE MAGNITUDE SQUARED OF THE VECTOR
;TIME APPROX 90 MICRO SEC
A←←4
↑SECOND:
HRL A,-1(P)
HRRI A,AC
BLT A,AC+2
FMPR AC,AC
FMPR AC+1,AC+1
FMPR AC+2,AC+2
FADR AC,AC+1
FADR AC,AC+2
SUB P,[2(2)]
JRST @2(P)
BEND
BEGIN ANGLE
;ANGLE(P,A,O) RETURNS THE ANGLE BETWEEN P AND A IN DEGREES ABOUT O
;A AND P MUST BE UNIT VECTORS. I.E. DIRECTION COSINES
;COMPUTES THE ANGLE=ACOS(PV1*AV1 + PV2*AV2 + PV3*AV3)
;AND THEN THE DETERMINANT
; | O1 O2 O3|
; |PV1 PV2 PV3|
; |AV1 AV2 AV3|
;AND IF POSITIVE MULTIPLIES THE ANGLE BY -1;
;TIME APPROX 550 MICRO SECONDS
TAC1←←2
PV1←←3
PV2←←4
PV3←←5
AV1←←6
AV2←←7
AV3←←10
O←←11
I←←12
↑ANGLE:
HRL AC,-3(P) ;MOVE PV INTO AC'S
HRRI AC,PV1
BLT AC,PV3
HRL AC,-2(P) ;MOVE AV INTO AC'S
HRRI AC,AV1
BLT AC,AV3
MOVEI I,2 ;COMPUTE DOT PRODUCT
MOVE AC,PV1
FMPR AC,AV1
L1: MOVE TAC1,PV1(I)
FMPR TAC1,AV1(I)
FADR AC,TAC1
SOJG I,L1
MOVEM AC,DOT
MOVE O,-1(P) ;COMPUTE DETERMINANT
MOVE AC,PV2
FMPR AC,AV3
MOVE TAC1,PV3
FMPR TAC1,AV2
FSBR AC,TAC1
FMPR AC,(O)
FMPR PV3,AV1
FMPR AV3,PV1
FSBR PV3,AV3
FMPR PV3,1(O)
FADR AC,PV3
FMPR PV1,AV2
FMPR AV1,PV2
FSBR PV1,AV1
FMPR PV1,2(O)
FADR AC,PV1
MOVEM AC,DET
PUSH P,DOT
PUSHJ P,ACOS
FMPR AC,RAD
SKIPL DET ;NEGATE ANGLE IF DET NEGATIVE
MOVN AC,AC
SUB P,[4(4)]
JRST @4(P)
DOT: 0
DET: 0
BEND
BEGIN SOLVE
;SOLVE(T,J) GIVEN THE TRANSFORM T, RETURNS THE SOLUTION IN "J"
;RETURNS FALSE IF THE SOLUTION IS NOT CORRECT DUE TO STOP LIMITS.
;TIME APPROX 4 M. SEC
TAC←←2
I←←3
↑SOLVE:
SETOM FLAG ;SET RETURN FLAG TRUE
SETZM FLIP ;SET TRUE IF JOINT 4 FLIPS
HRL AC,-1(P) ;INITIALIZE J
HRRI AC,J
BLT AC,J+5
;COMPUTE W=P-SHOLDER-A*S6
HRRZ AC,-2(P) ;MOVE P INTO MEMORY
MOVE TAC,3(AC)
MOVEM TAC,PV1
MOVE TAC,7(AC)
MOVEM TAC,PV1+1
MOVE TAC,13(AC)
MOVEM TAC,PV1+2
MOVE TAC,2(AC) ;MOVE A INTO MEMORY
MOVEM TAC,AV1
MOVE TAC,6(AC)
MOVEM TAC,AV1+1
MOVE TAC,12(AC)
MOVEM TAC,AV1+2
MOVE TAC,1(AC) ;MOVE O INTO MEMORY
MOVEM TAC,OV1
MOVE TAC,5(AC)
MOVEM TAC,OV1+1
MOVE TAC,11(AC)
MOVEM TAC,OV1+2
;J2=DOT(W,W)-S2↑2
MOVN TAC,S22 ;INITIALIZE TAC TO -S22
MOVEI I,2 ;COMPUTE THE DOT PRODUCT
L1: MOVE AC,AV1(I)
FMPR AC,S6
FADR AC,SHOLDER(I)
FSBR AC,PV1(I)
MOVEM AC,W(I) ;-W(I)
FMPR AC,AC
MOVEM AC,W2(I) ;W(I)↑2
FADR TAC,AC
SOJGE I,L1
JUMPLE TAC,[SETZ AC,
JRST NSR]
PUSH P,TAC
PUSHJ P,SQRT
NSR: CAMLE AC,STOP+3 ;CHECK THE STOPS
JRST[ SETZM FLAG
MOVE AC,STOP+3
JRST M3]
CAMGE AC,STOP+2
JRST[ SETZM FLAG
MOVE AC,STOP+2
M3: FADR AC,J+2
FSC AC,-1 ;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
JRST OK3]
OK3: MOVEM AC,J+2 ;JOINT THREE
;NOW COMPUTE TH FOR JOINT 1
PUSH P,W+1
PUSH P,W
PUSHJ P,ATAN2
JUMPL AC,ISP
FSBR AC,TPI
ISP: MOVEM AC,J
;NOW COMPUTE THE BOOM OFFSET
MOVE AC,W2+1
FADR AC,W2
PUSH P,AC
PUSHJ P,SQRT
MOVE TAC,S2
FDVR TAC,AC
CAML TAC,[1.0]
MOVSI TAC,(1.0)
PUSH P,TAC
PUSHJ P,ASIN
FADR AC,J ;ADD J
FMPR AC,RAD
CAMLE AC,STOP+1 ;AND CHECK THE STOPS FOR JOINT 1
JRST[ SETZM FLAG
MOVE AC,STOP+1
JRST M1]
CAMGE AC,STOP
JRST[ SETZM FLAG
MOVE AC,STOP
M1: FADR AC,J
FSC AC,-1 ;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
JRST OK1]
OK1: MOVEM AC,J ;JOINT ONE
;JOINT 2-ACOS(W3/J3)
MOVN AC,W+2
FDVR AC,J+2
MOVEM AC,CO2
PUSH P,AC
PUSHJ P,ACOS
FMPR AC,RAD
MOVNM AC,J+1 ;JOINT TWO
;WE NOW NEED SOME SINES AND COSINES
PUSH P,J
PUSHJ P,SIND
MOVEM AC,SI1
PUSH P,J
PUSHJ P,COSD
MOVEM AC,CO1
PUSH P,J+1
PUSHJ P,SIND
MOVEM AC,SI2
;WE NOW COMPUTE THE UNIT VECTORS AT THE END OF LINK 3
MOVE AC,CO1
FMPR AC,CO2
MOVEM AC,Y
MOVE AC,SI1
FMPR AC,CO2
MOVEM AC,Y+1
MOVE AC,SI2
MOVNM AC,Y+2
FMPR AC,CO1
MOVEM AC,Z
MOVE AC,SI1
FMPR AC,SI2
MOVEM AC,Z+1
MOVE AC,CO2
MOVEM AC,Z+2
;CROSS(VT,Z,A);
PUSH P,[VT]
PUSH P,[Z]
PUSH P,[AV1]
PUSHJ P,CROSS
;CHECK FOR DEGENERATE CASE
PUSH P,[VT]
PUSHJ P,SECOND
CAMG AC,[0.000001]
JRST DEGEN ;JOINTS 4 AND 6 ARE DEGENERATE
PUSH P,AC
PUSHJ P,SQRT ;GET MAGNITUDE OF VT
MOVEI I,2
FVT: MOVE TAC,VT(I)
FDVR TAC,AC
MOVEM TAC,VT(I)
SOJGE I,FVT
VTF: ;VT NOW UNIT MAGNITUDE
;NOW JOINT 4
PUSH P,[VT]
PUSH P,[Y]
PUSH P,[Z]
PUSHJ P,ANGLE
MOVE TAC,AC
FSBR TAC,J+3
CAMLE TAC,[180.0]
JRST[ FSBR AC,[360.0]
JRST TN]
CAMGE TAC,[-180.0]
JRST[ FADR AC,[360.0]
JRST TN]
TN:
MOVE TAC,AC
FSBR TAC,J+3
CAMLE TAC,[90.0]
JRST[ FSBR AC,[180.0]
JRST RVTO]
CAMGE TAC,[-90.0]
JRST[ FADR AC,[180.0]
RVTO: MOVNS VT
MOVNS VT+1
MOVNS VT+2
JRST SOK4]
SOK4:
;NOW CHECK THE STOPS
CAMLE AC,STOP+5
JRST[ FSBR AC,[180.0]
JRST RVT]
CAMGE AC,STOP+4
JRST[ FADR AC,[180.0]
RVT: MOVNS VT
MOVNS VT+1
MOVNS VT+2
SETOM FLIP
JRST OK4]
OK4: MOVEM AC,J+3 ;JOINT FOUR
;NOW JOINT 5
PUSH P,[AV1]
PUSH P,[Z]
PUSH P,[VT]
PUSHJ P,ANGLE
CAMLE AC,STOP+7
JRST[ SETZM FLAG
MOVE AC,STOP+7
JRST M5]
CAMGE AC,STOP+6
JRST[ SETZM FLAG
MOVE AC,STOP+6
M5: FADR AC,J+4
FSC AC,-1 ;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
JRST OK5]
OK5: MOVEM AC,J+4
;JOINT 6 IS NOW SIMPLE
PUSH P,[OV1]
PUSH P,[VT]
PUSH P,[AV1]
PUSHJ P,ANGLE
TOL: MOVE TAC,AC
FSBR TAC,J+5
CAML TAC,[180.0]
JRST[ FSBR AC,[360.0]
JRST TOL]
TOS: MOVE TAC,AC
FSBR TAC,J+5
CAMGE TAC,[-180.0]
JRST[ FADR AC,[360.0]
JRST TOS]
MOVEM AC,J+5
HRRZ AC,-1(P)
HRRZI TAC,(AC)
HRLI AC,J
BLT AC,5(TAC)
MOVE AC,FLAG
SUB P,[3(3)]
JRST @3(P)
DEGEN: SETZM J+4 ;JOINT 5 IS ZERO
PUSH P,[OV1]
PUSH P,[Y]
PUSH P,[AV1]
PUSHJ P,ANGLE
FSBR AC,J+3 ;LEAVE JOINT 4 AS IS
JRST TOL ;AND PROCEED AS USUAL
FLAG: 0
FLIP: 0
PV1: BLOCK 3
AV1: BLOCK 3
OV1: BLOCK 3
W: 0
SI1: 0
CO1: 0
W2: 0
SI2: 0
CO2: 0
VT: BLOCK 3
Y: BLOCK 3
Z: BLOCK 3
J: BLOCK 6
S2: S2V
S22: S22V
S6: S6V
TPI: 203622077325
STOP: -185.0
60.0
6.5
27.5
-175.0
140.0
-101.0
101.0
BEND
BEGIN UPDATE
;UPDATE(T,J)
;GIVEN JOINT ANGLES J COMPUTES THE TRANSFORMATION T
;TIME APPROX 6 M. SEC
A1←←2
A2←←3
A3←←4
R1←←5
R2←←6
R3←←7
I←←10
J←←11
K←←12
↑UPDATE:
HRL AC,-1(P) ;TH
HRRI AC,TH
BLT AC,TH+5
SETZM T ;SET T TO UNIT MATRIX
MOVE AC,[XWD T,T+1]
BLT AC,T+13
MOVSI AC,(1.0)
MOVEM AC,T
MOVEM AC,T+5
MOVEM AC,T+12
SETZM LINK
MATMUL: ;MATMUL T(K)←T(K-1)*A(K)
MOVE AC,[XWD T,TN]
BLT AC,TN+13 ;MOVE T INTO TN
MOVE K,LINK
CAIN K,2
JRST[ MOVE AC,SI3
MOVEM AC,SI
MOVE AC,CO3
MOVEM AC,CO
MOVE AC,TH+2
MOVEM AC,S+2
JRST IDSU]
PUSH P,TH(K)
PUSHJ P,SIND
MOVEM AC,SI
MOVE K,LINK
PUSH P,TH(K)
PUSHJ P,COSD
MOVEM AC,CO
IDSU: MOVE K,LINK
;FIRST ROW
MOVE A1,CO
MOVE A2,SI
SETZ A3,
MOVEI I,0
PUSHJ P,MUL
;SECOND ROW
MOVN A1,CA(K)
FMPR A1,SI
MOVE A2,CA(K)
FMPR A2,CO
MOVE A3,SA(K)
MOVEI I,1
PUSHJ P,MUL
;THIRD ROW
MOVE A1,SA(K)
FMPR A1,SI
MOVN A2,SA(K)
FMPR A2,CO
MOVE A3,CA(K)
MOVEI I,2
PUSHJ P,MUL
;FOURTH ROW
SETZ A1,
SETZ A2,
MOVE A3,S(K)
MOVEI I,3
PUSHJ P,MUL
MOVE AC,TN+3 ;OLD X
FADRM AC,T+3 ;NEW X
MOVE AC,TN+7 ;OLD Y
FADRM AC,T+7 ;NEW Y
MOVE AC,TN+13 ;OLD Z
FADRM AC,T+13 ;NEW Z
AOS K,LINK
CAIGE K,6
JRST MATMUL
HRRZ K,-2(P)
MOVEI AC,13(K)
HRLI K,T
BLT K,(AC)
HRRZ K,-2(P)
MOVE AC,SHOLDER
FADRM AC,3(K) ;X
MOVE AC,SHOLDER+1
FADRM AC,7(K) ;Y
MOVE AC,SHOLDER+2
FADRM AC,13(K) ;Z
SUB P,[3(3)]
JRST @3(P)
MUL: MOVEI J,2
L2: MOVE AC,PW(J)
BLT AC,R1+2
FMPR R1,A1
FMPR R2,A2
FMPR R3,A3
FADR R1,R2
FADR R1,R3
MOVEM R1,T(I)
ADDI I,4
SOJGE J,L2
POPJ P,
TH: BLOCK 6
PW: XWD TN+10,R1
XWD TN+4,R1
XWD TN,R1
LINK: 0
SI: 0
CO: 0
T: BLOCK 14
TN: BLOCK 14
SI3: -1.0
CO3: 0.0
S: 0.0
S2V
0.0
0.0
0.0
S6V
SA: -1.0
1.0
0.0
-1.0
1.0
0.0
CA: 0.0
0.0
1.0
0.0
0.0
1.0
BEND
BEGIN TRANSMULT
S←1
M←2
A1←3
A2←4
A3←5
A4←6
I←7
J←10
R←11
T←14
U←13
↑TIMES:HRL T,-1(P)
HRRI T,MLT
BLT T,MLT+13
MOVEI T,MLT
HRRZ U,-2(P)
HRRZ R,-3(P)
MOVEI I,3
L1: MOVEI J,3
MOVE A1,(U)
MOVE A2,1(U)
MOVE A3,2(U)
L2: MOVE S,A1
FMPR S,(T)
MOVE M,A2
FMPR M,4(T)
FADR S,M
MOVE M,A3
FMPR M,10(T)
FADR S,M
MOVEM S,(R)
AOJ R,
AOJ T,
SOJG J,L2
FMPR A1,(T)
FMPR A2,4(T)
FADR A1,A2
FMPR A3,10(T)
FADR A1,A3
FADR A1,3(U)
MOVEM A1,(R)
AOJ R,
SUBI T,3
ADDI U,4
SOJG I,L1
SUB P,[XWD 4,4]
JRST @4(P)
MLT: BLOCK 14
BEND
END